#!/usr/bin/python
# -*- coding: UTF-8 -*-
import time

#休眠毫秒数
def msSleep(ms):
    time.sleep(0.001 * ms)

#双足/双臂机器人
class BipeRobot:
    ##各种时间定义
    TIME_MOVING_STANDING = 1000 #双足模式前进时,轮子的旋转时间
    LITTlE_DELAY = 100          #设置硬件所需的等待延时
    TRANSFORMING_DELAY = 1000   #变换姿态所需的等待延时
    
    SPEED_LEG_WHEEL = 75              #双足模式时的轮子速度 150/200 STEP
    SPEED_LEG_JOINT = 75              #双足模式的关节速度 150 STEP
    SPEED_JOINT_TRANSFORMING = 40     #轮足转换的关节速度 80 STEP
    SPEED_WHEEL_TRANSFORMING = 75     #轮足转换的轮子速度 150 STEP
    SPEED_WHEELING_MODE_DEFAULT = 75  #轮式模式下的默认速度 150 STEP
    
    ##姿态位置角度
    ANGLE_MID_INIT = 0         #150; 512
    ANGLE_LEFT_TOP_INIT = 90   #240;
    ANGLE_RIGHT_TOP_INIT = 90  #240; 818

    ANGLE_LEFT_WHEEL_INIT = 0
    ANGLE_RIGHT_WHEEL_INIT = 0
    
    ANGLE_LEFT_MID_MOVE = 10   # (170 / CW_CCW_ANGLE_FACTOR) =545; //160-150
    ANGLE_RIGHT_MID_MOVE = -10 # (130 / CW_CCW_ANGLE_FACTOR) 477;//140-150
    ANGLE_MID_STAND = 0        # 150-150
    
    #ID 列表
    LEFT_WHEEL = 0
    LEFT_MID = 0
    LEFT_TOP = 0
    RIGHT_TOP = 0
    RIGHT_MID = 0
    RIGHT_WHEEL = 0
  
    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,mclib):
        self.lib = mclib

    #绑定ID： T T IU (F) T T IU
    def bindIds(self,ids):
       self.LEFT_TOP = ids[0]
       self.LEFT_MID = ids[1]
       self.LEFT_WHEEL = ids[2]
       self.RIGHT_TOP = ids[3]
       self.RIGHT_MID = ids[4]
       self.RIGHT_WHEEL = ids[5]


   ######################################################################
   ############################# 简单双足模式 ###########################
   ######################################################################
    #初始化为双足模式
    def initWalkingMode(self):
        #两个车轮变成速度模式
        self.lib.SetMotorMode(self.LEFT_WHEEL,1)
        self.lib.SetMotorMode(self.RIGHT_WHEEL,1)

        #使能两轮子
        self.lib.EnableMotor(self.LEFT_WHEEL,1)
        self.lib.EnableMotor(self.RIGHT_WHEEL,1)

        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)

        #设置角度
        ids = [self.LEFT_MID,self.LEFT_TOP,self.RIGHT_TOP,self.RIGHT_MID]
        angles = [self.ANGLE_MID_INIT,self.ANGLE_LEFT_TOP_INIT,self.ANGLE_RIGHT_TOP_INIT,self.ANGLE_MID_INIT]
        speeds = [75,75,75,75]
        self.lib.SetMotorPositionSyncWithSpeeds(ids,angles,speeds)

        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)

        #设置I轮子模块速度为0
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,0)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,0)

         #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)
        pass

    #双足-往前走
    def goForward_bipe(self):
        #立正
        self.initWalkingMode()

        #第一次移动时,左脚迈一半步子
	    #左脚移动
        self.lib.SetPositionWithSpeed(self.LEFT_MID, self.ANGLE_LEFT_MID_MOVE, self.SPEED_LEG_JOINT)
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)

        #左侧舵机逆时针(站立模式从上向下看)
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL, self.SPEED_LEG_WHEEL)
        #等待以让左脚行走一定时间
        msSleep(self.TIME_MOVING_STANDING)
        #停止左轮转动行走
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL, 0)

        #立正
        self.initWalkingMode()        
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)


        #右脚移动
        self.lib.SetPositionWithSpeed(self.RIGHT_MID, self.ANGLE_RIGHT_MID_MOVE, self.SPEED_LEG_JOINT)
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)

        #右侧舵机逆时针(站立模式从上向下看)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL, self.SPEED_LEG_WHEEL)
        #等待以让右脚行走一定时间
        msSleep(self.TIME_MOVING_STANDING)
        #停止右轮转动行走
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL, 0)

        #立正
        self.initWalkingMode()        
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)

        pass 

    #双足-往后走
    def goBack_bipe(self):
        #立正
        self.initWalkingMode()   
        
        #右脚移动
        self.lib.SetPositionWithSpeed(self.RIGHT_MID, self.ANGLE_RIGHT_MID_MOVE, self.SPEED_LEG_JOINT)
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)
        #右侧舵机旋转以行走
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL, self.SPEED_LEG_WHEEL * -1)
        #等待以让右脚行走一定时间
        msSleep(self.TIME_MOVING_STANDING)
        #停止右轮转动行走
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL, 0)
         #立正
        self.initWalkingMode()        
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)


        #左脚移动
        self.lib.SetPositionWithSpeed(self.LEFT_MID, self.ANGLE_LEFT_MID_MOVE, self.SPEED_LEG_JOINT)
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)

        #左侧舵机旋转以行走
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL, self.SPEED_LEG_WHEEL * -1)
        #等待以让左脚行走一定时间
        msSleep(self.TIME_MOVING_STANDING)
        #停止左轮转动行走
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL, 0)

        #立正
        self.initWalkingMode()        
        #等待以让硬件响应完成
        msSleep(self.LITTlE_DELAY)
        pass 


   ######################################################################
   ############################# 简单双轮模式 ###########################
   ######################################################################
   #转换为双轮模式
    def transToWheelMode(self):
        #设置角度
        ids = [self.LEFT_MID,self.LEFT_TOP,self.RIGHT_TOP,self.RIGHT_MID]
        angles = [self.ANGLE_MID_INIT,self.ANGLE_LEFT_TOP_INIT,self.ANGLE_RIGHT_TOP_INIT,self.ANGLE_MID_INIT]
        speeds = [75,75,75,75]
        self.lib.SetMotorPositionSyncWithSpeeds(ids,angles,speeds)

         #停止两轮转动
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL, 0)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL, 0)

        #P1
        self.lib.SetPositionWithSpeed(self.LEFT_MID, -50, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)
        self.lib.SetPositionWithSpeed(self.RIGHT_MID, -6, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P2
        self.lib.SetPositionWithSpeed(self.LEFT_MID, -70, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)
        self.lib.SetPositionWithSpeed(self.RIGHT_MID, -11, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P3
        self.lib.SetPositionWithSpeed(self.LEFT_MID, -80, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)
        self.lib.SetPositionWithSpeed(self.RIGHT_MID, -25, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P4
        self.lib.SetPositionWithSpeed(self.LEFT_TOP, 50, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)
        self.lib.SetPositionWithSpeed(self.RIGHT_MID, -35, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P5
        self.lib.SetPositionWithSpeed(self.LEFT_TOP, 20, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P6
        self.lib.SetPositionWithSpeed(self.RIGHT_TOP, 20, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P7
        self.lib.SetPositionWithSpeed(self.LEFT_TOP,0, self.SPEED_JOINT_TRANSFORMING)
        self.lib.SetPositionWithSpeed(self.RIGHT_MID,80, self.SPEED_JOINT_TRANSFORMING)
        #等待以让硬件响应完成
        msSleep(self.TRANSFORMING_DELAY)

        #P10
        #两个车轮变成速度模式
        self.lib.SetMotorMode(self.LEFT_WHEEL,1)
        self.lib.SetMotorMode(self.RIGHT_WHEEL,1)
        self.lib.SetMotorPosition(self.RIGHT_TOP,0)
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL, self.SPEED_WHEEL_TRANSFORMING)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL, self.SPEED_WHEEL_TRANSFORMING)

        self.lib.SetMotorPosition(self.LEFT_MID,0)
        self.lib.SetMotorPosition(self.RIGHT_MID,0)
        self.lib.SetMotorPosition(self.LEFT_MID,20)
        self.lib.SetMotorPosition(self.RIGHT_MID,-20)

        msSleep(2.5 * self.TRANSFORMING_DELAY)

        #P12
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,0)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,0)

        self.lib.SetMotorPosition(self.LEFT_TOP,90)
        self.lib.SetMotorPosition(self.RIGHT_TOP,90)
        self.lib.SetMotorPosition(self.LEFT_MID,-90)
        self.lib.SetMotorPosition(self.RIGHT_MID,-90)

        msSleep(self.TRANSFORMING_DELAY)
        pass

    #双轮-往前走
    def goForward_wheel(self):
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT * -1)
        pass

    #双轮-往后走
    def goBack_wheel(self):
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT * -1)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT)

        pass

    #双轮-左自转
    def turnLeft_wheel(self):
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT * -1)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT * -1)

        pass

    #双轮-右自转
    def turnRight_wheel(self):
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,self.SPEED_WHEELING_MODE_DEFAULT)

        pass

    #双轮-停止
    def stop_wheel(self):
        self.lib.SetMotorTargetSpeed(self.LEFT_WHEEL,0)
        self.lib.SetMotorTargetSpeed(self.RIGHT_WHEEL,0)
        pass


